#include "Scene/Physics.h"
#include "Scene/Components.h"

namespace CC
{

    RigidBody Physics::CreateShapeBox(float x, float y, float z, float mass, glm::vec3 trans, glm::vec3 rotation, glm::vec3 offset)
    {
        RigidBody rigidBody;

        btCollisionShape *shape = new btBoxShape(btVector3(btScalar(x), btScalar(y), btScalar(z)));

        btTransform transform;
        transform.setIdentity();
        transform.setOrigin(btVector3(trans[0] + offset[0], trans[1] + offset[1], trans[2] + offset[2]));

        btQuaternion orn;
        orn.setEulerZYX((rotation[2]), (rotation[1]), (rotation[0]));
        transform.setRotation(orn);

        btScalar _mass(mass);

        bool isDynamic = (_mass != 0.f);

        btVector3 localInertia(0, 0, 0);
        if (isDynamic)
            shape->calculateLocalInertia(_mass, localInertia);

        btDefaultMotionState *myMotionState = new btDefaultMotionState(transform);
        btRigidBody::btRigidBodyConstructionInfo rbInfo(_mass, myMotionState, shape, localInertia);
        btRigidBody *body = new btRigidBody(rbInfo);

        rigidBody.BtCollisionShape = shape;
        rigidBody.BtRigidBody = body;
        rigidBody.Mass = mass;
        rigidBody.BoxHalfExtents = glm::vec3(x, y, z);
        rigidBody.Transform = trans;
        rigidBody.Rotation = rotation;
        rigidBody.Offset = offset;
        rigidBody.RigidBodyShapeTypeValue = RigidBodyShapeType_Box;

        return rigidBody;
    }

    RigidBody Physics::CreateShapeTriangleMesh(int meshSize, floatPtr* vertexBase, int *numVertices, uShortPtr *indexBase, int *numIndexes, float mass, 
        glm::vec3 trans, glm::vec3 rotation, glm::vec3 scale, glm::vec3 offset)
    {
        RigidBody rigidBody;

        btTransform transform;
        transform.setIdentity();
        transform.setOrigin(btVector3(trans[0] + offset[0], trans[1] + offset[1], trans[2] + offset[2]));

        btQuaternion orn;
        orn.setEulerZYX((rotation[2]), (rotation[1]), (rotation[0]));
        transform.setRotation(orn);

        for (int i = 0; i < meshSize; i++)
        {
            btTriangleIndexVertexArray *meshInterface = new btTriangleIndexVertexArray();
            btIndexedMesh part;

            part.m_vertexBase = (const unsigned char *)vertexBase[i];
            part.m_vertexStride = sizeof(btScalar) * 3;
            part.m_numVertices = numVertices[i];
            part.m_triangleIndexBase = (const unsigned char *)indexBase[i];
            part.m_triangleIndexStride = sizeof(short) * 3;
            part.m_numTriangles = numIndexes[i] / 3;
            part.m_indexType = PHY_SHORT;

            meshInterface->addIndexedMesh(part, PHY_SHORT);

            bool useQuantizedAabbCompression = true;
            btBvhTriangleMeshShape *shape = new btBvhTriangleMeshShape(meshInterface, useQuantizedAabbCompression);

            btScalar _mass(mass);

            bool isDynamic = (_mass != 0.f);

            btVector3 localInertia(0, 0, 0);
            //if (isDynamic)
            //     shape->calculateLocalInertia(_mass, localInertia);

            btDefaultMotionState *myMotionState = new btDefaultMotionState(transform);

            btRigidBody::btRigidBodyConstructionInfo rbInfo(_mass, myMotionState, shape, localInertia);
            btRigidBody *body = new btRigidBody(rbInfo);
            body->setFriction(btScalar(0.9));

            rigidBody.BtRigidBodyes.push_back(body);
            rigidBody.BtCollisionShapes.push_back(shape);
        }

        rigidBody.Mass = mass;
        rigidBody.Transform = trans;
        rigidBody.Rotation = rotation;
        rigidBody.Scale = scale;
        rigidBody.Offset = offset;
        rigidBody.VertexBase = vertexBase;
        rigidBody.numVertices = numVertices;
        rigidBody.indexBase = indexBase;
        rigidBody.numIndexes = numIndexes;

       if (!rigidBody.MeshModelMaterialComponentDebug) {
            rigidBody.MeshModelMaterialComponentDebug = new MeshModelMaterialComponent(false, MaterialMesh(glm::vec4(0.0)));
        }

        ((MeshModelMaterialComponent*)rigidBody.MeshModelMaterialComponentDebug)->HasColorMaterial = true;
        ((MeshModelMaterialComponent*)rigidBody.MeshModelMaterialComponentDebug)->EanbleLight = false;

        if (!rigidBody.MeshComponentDebug) {
            rigidBody.MeshComponentDebug = (void*)(new MeshComponent(Mesh()));
        }
        
        for (int i = 0; i < numVertices[0]; i++) {
            Vertex v;
            v.Position = glm::vec3( vertexBase[0][i*3], vertexBase[0][i*3 + 1], vertexBase[0][i*3 + 2] );
            ((MeshComponent*)rigidBody.MeshComponentDebug)->MeshData.Vertices.push_back(v);

        }
        for (int i = 0; i < numIndexes[0]; i++) {
            ((MeshComponent*)rigidBody.MeshComponentDebug)->MeshData.Indices.push_back(indexBase[0][i]);
        }

        rigidBody.RigidBodyShapeTypeValue = RigidBodyShapeType_TriangleMesh;

        return rigidBody;
    }

    void Physics::SetTransform(RigidBody &rigidBody, glm::vec3 trans)
    {
        btTransform _trans;
        _trans.setIdentity();

        _trans.setOrigin(btVector3(trans[0], trans[1], trans[2]));

        btRigidBody *body = rigidBody.BtRigidBody;
        body->setWorldTransform(_trans);
    }
}